#ifndef VEX_FUNCTIONS_H
#include "Vex_Functions.h"
#endif

#ifndef VEX_FUNCTIONS_C
#include "Vex_Functions.c"
#endif

#define AUTONOMOUS_C

#define ARMACCEPTANCEDISTANCE 30




 //=======================Autonomous Godzilla=================================
int stage = 0;
int autoCounter;
void godzilla(bool red){
armSampleUpdate();

    while(autoCounter<10000)
       {

        drive(127,127);
        autoCounter++;
       }

       if( autoCounter > 9999){
         drive(0, 0);
       }




     /*if (stage == 0){
      wait1Msec(17000);
     if(driveStraight(100)){
      stage++;
    }
   }*/


   /*
   if (stage == 0){
	    liftArm(medArmHeight);
	    if (withinRange (armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE)){
	      stage++;
	    }
   }

   if (stage == 1){
     liftArm(medArmHeight);
     if(driveStraight(-100)){
      stage++;
    }
   }
   if (stage == 2){
     liftArm(medArmHeight);
     if(driveTurn(red?-90:90)){
      stage++;
    }
  }

    if (stage == 3){
     liftArm(medArmHeight);
     if(driveStraight(100)){
      stage++;
    }
   }

	 if (stage == 4){
	    liftArm(lowArmHeight);
	    if (withinRange (armAverage(),lowArmHeight,ARMACCEPTANCEDISTANCE)){
	      stage++;
	    }
	 }

   if (stage == 5){
      liftArm(lowArmHeight);
      motor[port8] = grabberServoOpen;
      wait1Msec(500);
      stage++;
   }

   if (stage == 6){
      liftArm(medArmHeight);
      if (withinRange (armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE)){
        stage++;
      }
   }

    if (stage == 7){
     liftArm(medArmHeight);
     if(driveTurn(red?45:-45)){
      stage++;
    }
  }

   if (stage == 8){
      liftArm(medArmHeight);
      if(driveStraight(50)){
      stage++;
    }
   }

   if (stage == 9){
      liftArm(medArmHeight);
      motor[port8] = grabberServoClose;
      wait1Msec(500);
      stage++;
   }

   if (stage == 10){
      liftArm(medArmHeight);
      if(driveStraight(-50)){
      stage++;
      }
   }


   if(stage == 11){
		  motor[port6]=0;
		  stage ++;
   }

   if (stage == 12){
      driveTurn(red?30:-30);{
      stage++;
    }
  }

   if (stage == 13){
      driveStraight(200);{
      stage++;
    }
   }*/

}




//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++













/*#ifndef VEX_FUNCTIONS_H
#include "Vex_Functions.h"
#endif

#ifndef VEX_FUNCTIONS_C
#include "Vex_Functions.c"
#endif

#define AUTONOMOUS_C

#define ARMACCEPTANCEDISTANCE 30




 //=======================Autonomous Godzilla=================================
int stage = 0;
int autoCounter = 0;
void godzilla(bool red){
armSampleUpdate();



  if (stage == 0){
     liftArm(medArmHeight);
     if(driveStraight(100)){
      /*stage++;
    }
   }

      if (stage == 2){
	    liftArm(medArmHeight);
	    if (withinRange (armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE)){
	      stage++;
	    }
   }


   if (stage == 2){
     liftArm(medArmHeight);
     if(driveTurn(red?-90:90)){
      stage++;
    }
  }

    if (stage == 3){
     liftArm(medArmHeight);
     if(driveStraight(100)){
      stage++;
    }
   }

	 if (stage == 4){
	    liftArm(lowArmHeight);
	    if (withinRange (armAverage(),lowArmHeight,ARMACCEPTANCEDISTANCE)){
	      stage++;
	    }
	 }

   if (stage == 5){
      liftArm(lowArmHeight);
      motor[port8] = grabberServoOpen;
      wait1Msec(500);
      stage++;
   }

   if (stage == 6){
      liftArm(medArmHeight);
      if (withinRange (armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE)){
        stage++;
      }
   }

    if (stage == 7){
     liftArm(medArmHeight);
     if(driveTurn(red?45:-45)){
      stage++;
    }
  }

   if (stage == 8){
      liftArm(medArmHeight);
      if(driveStraight(50)){
      stage++;
    }
   }

   if (stage == 9){
      liftArm(medArmHeight);
      motor[port8] = grabberServoClose;
      wait1Msec(500);
      stage++;
   }

   if (stage == 10){
      liftArm(medArmHeight);
      if(driveStraight(-50)){
      stage++;
      }
   }


   if(stage == 11){
		  motor[port6]=0;
		  stage ++;
   }

   if (stage == 12){
      driveTurn(red?30:-30);{
      stage++;
    }
  }

   if (stage == 13){
      driveStraight(200);{
      stage++;
    }
   }*/










 //=====================Autonomous Far Blue ===============================


 void auto(bool blue, bool close)


{
  armSampleUpdate();
  if (stage == 0){
    liftArm(lowArmHeight);
    averageheight = armAverage();
    if (      withinRange(averageheight,lowArmHeight,ARMACCEPTANCEDISTANCE)      ){
      stage++;
    }
  }

  if (stage == 1){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
      stage++;

  }


//TODO: the arm motors "remember" thier last value when the stage moves up to 3 etc...
//TODO:  The result is that they continue to turn with a low value after we have moved past this stage.


  if (stage == 2 && driveTurn(blue?-20:20)){


      stage++;

  }
  if (stage == 3 && driveStraight(100)){


      stage++;

  }
  if (stage == 2  || stage == 3 ||  stage == 4){
    liftArm(medArmHeight);
    if (withinRange (armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE) && stage == 4){
      stage++;
    }
  }



  if (stage == 5){
    motor[port8] = grabberServoClose;
    wait1Msec(500);
      stage++;

  }
  if (stage == 6){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
      stage++;

  }
  if (stage == 7 && driveTurn(180)){


      stage++;

  }

  if (stage == 8 && driveStraight(200)){


      stage++;

  }
  if (stage == 7 || stage == 8 || stage == 9){
    liftArm(highArmHeight);
    if (withinRange(armAverage(),highArmHeight,ARMACCEPTANCEDISTANCE)&& stage == 9){
      stage++;
    }
  }
  if (stage == 10){
   motor[port8] = grabberServoClose;
   wait1Msec(500);
      stage++;

  }
  if (stage == 11){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
     stage++;

  }
  if (stage == 12 && driveTurn(blue?-150:150)){


      stage++;

  }
  if (stage == 13 && driveStraight(300)){


      stage++;

  }
  if (stage == 12 || stage == 13 || stage == 14){
    liftArm(lowArmHeight);
    if (withinRange(armAverage(),lowArmHeight,ARMACCEPTANCEDISTANCE)&& stage == 14){
      stage++;
    }
  }
  if (stage == 15){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
     stage++;

  }


  if (stage == 16 && driveTurn(blue?-80:80)){


      stage++;

  }
   if (stage == 17 && driveStraight(100)){


      stage++;

  }
  if (stage == 16 || stage == 17 || stage == 18){
    liftArm(medArmHeight);
    if (withinRange(armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE)&& stage == 18){
      stage++;
    }
  }
  if (stage == 19){
    motor[port8] = grabberServoClose;
    wait1Msec(500);
      stage++;

  }

}


/* //=====================Autonomous Far Red ===============================

void farRed()


{
  if (stage == 0){
    liftArm(lowArmHeight);
    if (withinRange(armAverage(),lowArmHeight,ARMACCEPTANCEDISTANCE)){
      stage++;
    }
  }
  if (stage == 1){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
      stage++;

  }


  if (stage == 2 && driveTurn(20)){


      stage++;

  }
  if (stage == 3 && driveStraight(100)){


      stage++;

  }
  if (stage== 2 || stage== 3 || stage== 4){
    liftArm(medArmHeight);
    if (withinRange(armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE)&& stage==4){
      stage++;
    }
  }

if (stage == 5){
    motor[port8] = grabberServoClose;
    wait1Msec(500);
      stage++;

  }
  if (stage == 6){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
      stage++;

  }
  if (stage == 7 && driveTurn(180)){


      stage++;

  }

  if (stage == 8 && driveStraight(200)){


      stage++;

  }
   if (stage == 7 || stage == 8 || stage == 9){
    liftArm(highArmHeight);
    if (withinRange(armAverage(),highArmHeight,ARMACCEPTANCEDISTANCE)&& stage == 9){
      stage++;
    }
  }
  if (stage == 10){
   motor[port8] = grabberServoClose;
   wait1Msec(500);
      stage++;

  }
  if (stage == 11){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
     stage++;

  }
  if (stage == 12 && driveTurn(150)){


      stage++;

  }
  if (stage == 13 && driveStraight(300)){


      stage++;

  }
  if (stage == 12 || stage == 13 || stage == 14){
    liftArm(lowArmHeight);
    if (withinRange(armAverage(),lowArmHeight,ARMACCEPTANCEDISTANCE)&& stage == 14){
      stage++;
    }
  }
  if (stage == 15){
    motor[port8] = grabberServoOpen;
    wait1Msec(500);
     stage++;

  }


  if (stage == 16 && driveTurn(80)){


      stage++;

  }
  if (stage == 17 && driveStraight(100)){


      stage++;

  }
  if (stage == 16 || stage == 17 || stage == 18){
    liftArm(medArmHeight);
    if (withinRange(armAverage(),medArmHeight,ARMACCEPTANCEDISTANCE)&& stage == 18){
      stage++;
    }
  }
  if (stage == 19){
    motor[port8] = grabberServoClose;
    wait1Msec(500);
      stage++;

  }

}*/
